#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     HTMC,           sensorI2CCustom)
#pragma config(Motor,  motorA,          A,             tmotorNXT, openLoop, reversed)
#pragma config(Motor,  motorB,          B,             tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          C,             tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     A1,            tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     A2,            tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     A3,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     A4,            tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_1,     A5,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     A6,            tmotorTetrix, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"
#include "drivers/common.h"
#include "drivers/hitechnic-compass.h"
//#include "drivers/HTAC-driver.h"
//#include "drivers/HTGYRO-driver.h"


void buttons()
{
	getJoystickSettings(joystick);
  	if (joy1Btn(4)==1)
    {
      motor[A5]=50;
    }

    else if (joy1Btn(2)==1)
    {
      motor[A5]=-10;
    }
    else if (joy1Btn(1)==1)
    {
      motor[A6]=-10;
    }
    else if (joy1Btn(3)==1)
    {
      motor[A6]=50;;
    }
    else if (joy1Btn(8)==1)
    {
      motor[A]=100;
      motor[B]=100;
    }
    else if (joy1Btn(7)==1)
    {
      motor[A]=-100;
      motor[B]=-100;
    }

    else
    {
      motor[A5]=0;
      motor[A6]=0;
      motor[A]=0;
      motor[B]=0;
    }
}


float joyAngleAtan(int xPos, int yPos)
{
  float offset = 0;


  if(yPos < 0 && xPos < 0) {
    offset = 180;
  } else if(yPos < 0) {
    offset = -180;
  }

  if(yPos != 0) {
    return radiansToDegrees(atan(xPos/yPos)) + offset;
  } else {
     if(xPos > 0) {
      return 90;
      } else {
        return 270;
    }
  }
}

task main()
{

  float xJoy, yJoy, rJoy ;
  float joyAngle;
  int compassAngle;
  float driveSpeed1, driveSpeed2, driveSpeed3, driveSpeed4;
  float driveSpeedBaseMajor, driveSpeedBaseMinor;


  HTMCsetTarget(HTMC);


  while(true)
  {
    //Keep this blocked out
    /*
    motor[A1] = 0;
    motor[A2] = 0;
    motor[A3] = 0;
    motor[A4] = 0;
    */


    /*
    if(joy1Btn(2) == 1)
    {
      HTMCstartCal(HTMC);
      wait1Msec(1000);
    }

    if(joy1Btn(3) == 1)
    {
      HTMCstopCal(HTMC);
      wait1Msec(1000);
    }
    */


    int _debugTheta = HTMCreadRelativeHeading(HTMC);

    int _debugDrive = motor[A1];
    getJoystickSettings(joystick);

    //Recalibrate Header
    if(joy1Btn(5) == 1)
    {
      HTMCsetTarget(HTMC);
    }



    driveSpeed1 = 0;
    driveSpeed2 = 0;
    driveSpeed3 = 0;
    driveSpeed4 = 0;


    xJoy = joystick.joy1_x1;
    yJoy = joystick.joy1_y1;
    rJoy = sqrt(xJoy*xJoy + yJoy*yJoy);
    compassAngle = HTMCreadRelativeHeading(HTMC);

    //DRIVE
    if( rJoy > 20 )
    {

      driveSpeedBaseMajor = xJoy/rJoy * cosDegrees(135 - compassAngle) + yJoy/rJoy * sinDegrees(135 - compassAngle);
      driveSpeedBaseMinor = xJoy/rJoy * cosDegrees(45 - compassAngle) + yJoy/rJoy * sinDegrees(45 - compassAngle);

    } else {

      driveSpeedBaseMajor = 0;
      driveSpeedBaseMinor = 0;

    }

    //AUTO TURN


    //MANUAL TURN
    if( joystick.joy1_x2*joystick.joy1_x2 > 200 )
    {
      driveSpeed1 += joystick.joy1_x2 / 5;
      driveSpeed2 += -joystick.joy1_x2 / 5;
      driveSpeed3 += -joystick.joy1_x2 / 5;
      driveSpeed4 += joystick.joy1_x2 / 5;
    }

		if(joy1Btn(8)==1)
		{
			motor[A1] = 25*driveSpeedBaseMajor + driveSpeed1 / 2;
    	motor[A2] = 25*driveSpeedBaseMinor + driveSpeed2 / 2;
    	motor[A3] = 25*driveSpeedBaseMajor + driveSpeed3 / 2;
    	motor[A4] = 25*driveSpeedBaseMinor + driveSpeed4 / 2;
		}

 		else
 		{
    	motor[A1] = 50*driveSpeedBaseMajor + driveSpeed1;
    	motor[A2] = 50*driveSpeedBaseMinor + driveSpeed2;
    	motor[A3] = 50*driveSpeedBaseMajor + driveSpeed3;
    	motor[A4] = 50*driveSpeedBaseMinor + driveSpeed4;
		}
    //Debug
    if(joy1Btn(1) == 1)
    {
      nMotorEncoder[A1] = 0;
      nMotorEncoder[A2] = 0;
      nMotorEncoder[A3] = 0;
      nMotorEncoder[A4] = 0;
    }

    int _motor1 = nMotorEncoder[A1];
    int _motor2 = nMotorEncoder[A2];
    int _motor3 = nMotorEncoder[A3];
    int _motor4 = nMotorEncoder[A4];


    //JOY 2

   bFloatDuringInactiveMotorPWM = false;

       motor[A5] = joystick.joy2_y1;
   		 motor[A6] = joystick.joy2_y2;

	buttons();

   		 /*		if (joy2Btn(1)==1)
			{
				motor[A] = 100;
				motor[B] = 100;
			}
		  else if(joy2Btn(4)==1)
			{
	  		motor[A] = -100;
		 		motor[B] = -100;
		  }
		  else if(joy2Btn(8)==1)
		  {
		  	motor[A]=100;
		  	motor[B]=0;
		  }
		  else if(joy2Btn(6)==1)
		  {
		  	motor[A]=-100;
		  	motor[B]=0;
		  }
		  else if(joy2Btn(7)==1)
		  {
		  	motor[A]=0;
		  	motor[B]=100;
		  }
		  else if(joy2Btn(5)==1)
		  {
		  	motor[A]=0;
		  	motor[B]=-100;
		  }
		  else
		  {
		  	motor[A] = 0;
		  	motor[B] = 0;

		  if (joy2Btn(3) == 1)
		  {
		  	motor[C] = -10;
		  }
		  else if (joy2Btn(2) == 1)
		  {
		  	motor[C] = 10;
		  }
		  else
		  {
		  	motor[C] = 0;
		  }
		}

    //Platform
    /*if(joy1Btn(5) == 1)

    {
    	motor[A7] = 60;
    	motor[A8] = 60;
    } else if(joy1Btn(7) == 1) {
    	motor[A7] = -40;
    	motor[A8] = -40;
    } else {
    	motor[A7] = 0;
    	motor[A8] = 0;
    }
*/





  }
}
